#include <manual/input.h>

int kfd = 0;
struct termios cooked, raw;	


void Input::quit (int sig)
{
	tcsetattr(kfd, TCSANOW, & cooked);
	ros::shutdown();
	exit(0);
}


void Input::keyboard_loop ()
{
	char c;

	// get the console in raw mode                                                              
	tcgetattr(kfd, &cooked);
	memcpy(&raw, &cooked, sizeof(struct termios));
	raw.c_lflag &=~ (ICANON | ECHO);
	// Setting a new line, then end of file                         
	raw.c_cc[VEOL] = 1;
	raw.c_cc[VEOF] = 2;
	tcsetattr(kfd, TCSANOW, &raw);

	ROS_INFO("Reading from keyboard");
	ROS_INFO("---------------------------");
	ROS_INFO("Use arrow keys to move.");


	for(;;)
	{
		// get the next event from the keyboard  
		if(read(kfd, &c, 1) < 0)
		{
		  perror("read():");
		  exit(-1);
		}

		ROS_DEBUG("value: 0x%02X\n", c);
	      
		switch(c)
		{
		  case KEYCODE_L:
		    ROS_DEBUG("LEFT");
		    angle = 0.5;
		    break;
		  case KEYCODE_R:
		    ROS_DEBUG("RIGHT");
		    angle = -0.5;
		    break;
		  case KEYCODE_U:
		    ROS_DEBUG("UP");
		    angle = 0.0;
		    break;
		  case KEYCODE_D:
		    ROS_DEBUG("DOWN");
		    angle = 1.0;
		    break;
		}
		boost::mutex::scoped_lock lock(publish_mutex_);
		last_publish_ = ros::Time::now();
	    publish(std::abs(angle) > 0.01 ? 0.0 : 1.0, angle * M_PI);
		//publish(0.0, angle * 2.0 * M_PI);
		//publish(1.0, 0.0);
	}
	return;
}


